#include "camControl/camera.h"
#include "flycapture/FlyCapture2.h"

using namespace std;

void
camCleanUp(FlyCapture2::Camera *cam, FlyCapture2::Error error) {

    cout << error.GetDescription() << endl;

    error = cam->StopCapture();
    if (error != FlyCapture2::PGRERROR_OK)
        cout << error.GetDescription() << endl;

    error = cam->Disconnect();
    if (error != FlyCapture2::PGRERROR_OK)
        cout << error.GetDescription() << endl;

    error.PrintErrorTrace();
}

bool
camSetCarateristic(FlyCapture2::Camera *cam, FlyCapture2::Error *error, FlyCapture2::Format7ImageSettings *fmtImageSettings) {

    FlyCapture2::Format7PacketInfo fmtPacketInfo;
    bool valid;

    *error = cam->ValidateFormat7Settings(fmtImageSettings, &valid, &fmtPacketInfo);
    if (*error != FlyCapture2::PGRERROR_OK)
        return false;

    if (!valid) return false;

    *error = cam->SetFormat7Configuration(fmtImageSettings,
            fmtPacketInfo.recommendedBytesPerPacket);

    if (*error != FlyCapture2::PGRERROR_OK)
        return false;

    return true;
}

bool
camOpenConfig(FlyCapture2::Camera *cam, FlyCapture2::Error *error) {

    unsigned int numCameras;
    FlyCapture2::BusManager busMgr;

    *error = busMgr.GetNumOfCameras(&numCameras);
    if (!numCameras) {
        cout << "No cameras found" << endl;
        return false;
    }

    cout << "There were " << numCameras << " cameras found attached to your PC" << endl;
    FlyCapture2::PGRGuid *guid = new FlyCapture2::PGRGuid[numCameras];
    unsigned int index = 0;

    while (index < numCameras) {
        *error = busMgr.GetCameraFromIndex(index, &guid[index]);
        if (*error != FlyCapture2::PGRERROR_OK)
            return false;
        index++;
    }

    cout << "Working with the first camera" << endl;
    *error = cam->Connect(&guid[0]);
    if (*error != FlyCapture2::PGRERROR_OK)
        return false;

    FlyCapture2::Format7ImageSettings fmtImageSettings;
    fmtImageSettings.mode = FlyCapture2::MODE_0;
    fmtImageSettings.pixelFormat = FlyCapture2::PIXEL_FORMAT_RAW8;
    fmtImageSettings.offsetX = 328;
    fmtImageSettings.offsetY = 242;
    fmtImageSettings.width = 640;
    fmtImageSettings.height = 480;

    camSetCarateristic(cam, error, &fmtImageSettings);

    *error = cam->StartCapture();
    if (*error != FlyCapture2::PGRERROR_OK)
        return false;

    return true;
}

bool
CaptureImage(FlyCapture2::Image *RGBImage, FlyCapture2::Image *BayerImage, FlyCapture2::Camera *cam, FlyCapture2::Error *error) {

    *error = cam->RetrieveBuffer(BayerImage);
    if (*error != FlyCapture2::PGRERROR_OK)
        return false;

    FlyCapture2::Image copyImage;
    *error = copyImage.DeepCopy(BayerImage);
    if (*error != FlyCapture2::PGRERROR_OK)
        return false;

    *error = copyImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, RGBImage);
    if (*error != FlyCapture2::PGRERROR_OK)
        return false;
    return true;
}
